
#include "Moose.h"
#include "RayLine.h"
#include "SideElement.h"
#include "libmesh/vector_value.h"

RayLine::RayLine() :
	LineSegment(Point(0, 0, 0), Point(0, 0, 0)),
	_lenght(0)

{
}

RayLine::RayLine(const Point& p0, const Point& p1, Real lenght) :
	LineSegment(p0, p1),
	_normal((p1 - p0).unit()),
	_p0(p0),
	_p1(p1),
	_lenght(lenght)
{
}

RayLine::RayLine(const Point& p0, const Point& normal) :
	LineSegment(p0, p0 + normal * RaylineMax),
	_normal(normal),
	_p0(p0),
	_p1(p0 + normal * RaylineMax),
	_lenght(RaylineMax)
{
}

RayLine::RayLine(const Point& p0, const Point& p1, int i) :
	LineSegment(p0, p1),
	_normal((p1 - p0).unit()),
	_p0(p0),
	_p1(p1),
	_lenght((p1 - p0).norm())
{
}

bool RayLine::sideIntersectedByLine(const Elem* side, Point& point)
{
	bool inter(false);
	unsigned int dim = side->dim();
	if (dim == 2)
	{
		Plane plane(side->point(0), side->point(1), side->point(2));
		inter = intersect(plane, point);
	}
	else if (dim == 1)
	{
		RayLine side_ray(side->point(0), side->point(1), 1);
		inter = intersect(side_ray, point);
	}

	else // 1D
	{
		inter = contains_point(side->point(0));
		if (inter)
			point = side->point(0);
	}

	if (side->contains_point(point))
		return inter;

	else
		return false;
}

bool RayLine::sideIntersectedByLine(SideElement* side, Point& point)
{
	return sideIntersectedByLine(side->_elem, point);
}

bool RayLine::intersect(const Plane& pl, Point& intersect_p) const
{
	/**
	 * There are three cases in 3D for intersection of a line and a plane
	 * Case 1: The line is parallel to the plane - No intersection
	 *         Numerator = non-zero
	 *         Denominator = zero
	 *
	 * Case 2: The line is within the plane - Inf intersection
	 *         Numerator = zero
	 *         Denominator = zero
	 *
	 * Case 3: The line intersects the plane at a single point
	 *         Denominator = non-zero
	 */

	Point pl0 = pl.get_planar_point();
	RealVectorValue N = pl.unit_normal(_p0);
	RealVectorValue I = (_p1 - _p0).unit();

	Real numerator = (pl0 - _p0) * N;
	Real denominator = I * N;

	// The Line is parallel to the plane
	if (std::abs(denominator) < RaylineZero)
	{
		// The Line is on the plane
		if (std::abs(numerator) < RaylineZero)
		{
			// The solution is not unique so we'll just pick an end point for now
			intersect_p = _p0;
			return true;
		}
		return false;
	}

	Real d = numerator / denominator;

	// Make sure we haven't moved off the line segment!
	if (d + RaylineZero < 0 || d - RaylineZero >(_p1 - _p0).norm())
		return false;

	intersect_p = d * I + _p0;

	return true;
}

bool RayLine::intersect(const RayLine& l, Point& intersect_p) const
{
	/**
	 * First check for concurance:
	 *
	 *
	 * | x1 y1 z1 1 |
	 * | x2 y2 z2 1 | = (x3 - x1) * [(x2-x1) x (x4-x3)] = 0
	 * | x3 y3 z3 1 |
	 * | x4 y4 z4 1 |
	 *
	 *
	 * Solve:
	 *   x = _p0 + (_p1 - _p0)*s
	 *   x = l.p0 + (l._p1 - l.p0)*t
	 *
	 *   where
	 *   a = _p1 - _p0
	 *   b = l._p1 - l._p0
	 *   c = l._p0 - _p0
	 *
	 *   s = (c x b) * (a x b) / | a x b |^2
	 */
	RealVectorValue a = _p1 - _p0;
	RealVectorValue b = l._p1 - l._p0;
	RealVectorValue c = l._p0 - _p0;

	RealVectorValue v = a.cross(b);

	// Check for parallel lines
	if (std::abs(v.norm()) < RaylineZero && std::abs(c.cross(a).norm()) < RaylineZero)
	{
		return true;
	}

	// Check that the lines are coplanar
	Real concur = c * (a.cross(b));
	if (std::abs(concur) > RaylineZero)
		return false;

	Real s = (c.cross(b) * v) / (v * v);
	Real t = (c.cross(a) * v) / (v * v);

	if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
	{
		intersect_p = _p0 + s * a;
		return true;
	}
	return false;
}
